#pragma once
#include <ros/ros.h>
#include "control/custom_class.h"
#include "control/control_params.h"
#include "common_lib/log.h"
#include "common_lib/common.h"
#include "amr_trajectory_msgs/amr_trajectory_msg.h"
#include "control/move_control/move_fsm.h"
#include "control/move_control/path_following_base.h"
#include "control/move_control/trajectory_process/trajectory_process_base.h"
#include "control/move_control/trajectory_process/trajectory.h"
#include "control/move_control/lqr.h"
#include "control/move_control/cfc.h"

#include "map_operation_msgs/GetNearestInfo.h"

namespace control
{
    class MoveControl
    {
    public: 
        MoveControl(ros::NodeHandle &nh, ros::NodeHandle &private_nh);

        void Init(std::shared_ptr<AMRState> ptr_amr, std::shared_ptr<control_msgs::control_msg> ptr_control);
        ~MoveControl();

        void Process(void);
        
        //外部设置最大速度和最大角速度
        void SetSpeedLimit(float max_line_speed, float max_angle_speed);
        void SetSpeedLimit(float max_line_speed, float max_angle_speed, bool stop_reset_speed);

        //获得上装控制模块当前执行的任务id和任务状态
        void GetTaskInfo(uint32_t &task_id, FsmState &task_state, ros::Time &task_time);

        // set pub outside speed mode
        bool SetOutSideSpeedMode(bool use_out_mode);

        // pub outside speed
        void SetOutSideSpeed(float speed, float angle_speed, float speed_toward);

        void SetForceFinsih(bool flag);

    private: 
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
        
        common_lib::Common com_;
        std::string target_point_id_ = "";
        std::shared_ptr<AMRState> ptr_amr_state_;
        std::shared_ptr<control_msgs::control_msg> ptr_ctrl_out_;
        amr_trajectory_msgs::amr_trajectory_msg msg_task_;
        
        std::shared_ptr<FSM::MoveFsm> ptr_move_fsm_;//底盘控制状态机

        std::shared_ptr<PathFollowingBase> ptr_path_following_;
        std::shared_ptr<TrajectoryProcessBase> ptr_trajectory_process_;

        ros::ServiceClient srv_client_nearest_info_;

        //线程锁
        std::mutex mtx_trajectory;
        std::mutex mtx_trajectory_all;

        uint32_t task_id_ = 0;//task id
        uint32_t last_task_id_ = 0;
        long int last_min_index_ = -1;
        bool init_path_id_ = true;
        ros::Time search_timestamp_ = ros::Time(0, 0);
        geometry_msgs::Pose localization_record_;

        float max_speed_limit_ = 2.0f; 
        float max_angle_speed_limit_ = 90.0*M_PI/180.0f;

        //根据接收到的规划路径,更新ego_state_
        void UpdateTrajectory();

        bool GetPointInfoSrv(map_operation_msgs::GetNearestInfo &srv);

        //预处理
        void PreProcess();

        //提取局部路径
        void CalcTrajectoryPro();

        void RotateControl();//原地旋转控制        
        void PostProcess();//后处理模块（计算所在路点id、节点id、任务id）

        //发布数据
        void MsgTransfer();
    };
}